#include "test_local_planner.h"

LocalPlanner::LocalPlanner(tf2_ros::Buffer* tf_buffer):
    __planner_ptr(nullptr),__goal_pose_ptr(nullptr),__scan_ptr(nullptr){
    // 路径生成规划器
    int planner_type = ros::param::param<int>("/local_planner/planner_type", 0);
    switch(planner_type){
        case 0:
            __local_planner_type = LocalPlannerType::DWA;
            __planner_ptr = boost::make_shared<DWAPlanner>(tf_buffer);
            break;
        default:
            break;
    }
    __rviz_pose_sub = __nh.subscribe("/move_base_simple/goal", 10, &LocalPlanner::RvizPosesCallback, this);
    __scan_sub = __nh.subscribe("/scan_raw", 10, &LocalPlanner::LaserScanCallback, this);
    __goal_pose_pub = __nh.advertise<geometry_msgs::PoseStamped>("/global_goal_pose", 1, true);
    __base_scan_pub = __nh.advertise<sensor_msgs::LaserScan>("/base_scan", 1);

    if(!__planner_ptr) return;
    ros::Rate rate(20);     //控制频率
    string base_frame = "base_link";
    while(!ros::isShuttingDown()){
        rate.sleep();
        ros::spinOnce();
        // 获取雷达位姿
        if(!__scan_ptr) continue;

        // 获取雷达到机器人位姿变换数据
        geometry_msgs::PoseStamped base2laser_link;
        if(!__planner_ptr->GetTFPose(base2laser_link, base_frame, __scan_ptr->header.frame_id)){
            continue;
        }
        ROS_INFO("base2laser_link, x=%0.3f, y=%0.3f, yaw=%0.3f.", 
            base2laser_link.pose.position.x, 
            base2laser_link.pose.position.y,
            tf2::getYaw(base2laser_link.pose.orientation)
        );
        tf2::Transform base2laser_tf;
        tf2::convert(base2laser_link.pose, base2laser_tf);
        LidarScan2D lidatr_scan_2d;
        lidatr_scan_2d.frame_id = base_frame;
        lidatr_scan_2d.time_stamp = __scan_ptr->header.stamp;
        lidatr_scan_2d.angle_min = __scan_ptr->angle_min;
        lidatr_scan_2d.angle_max = __scan_ptr->angle_max;
        lidatr_scan_2d.angle_increment = __scan_ptr->angle_increment;
        lidatr_scan_2d.time_increment = __scan_ptr->time_increment;
        lidatr_scan_2d.scan_time = __scan_ptr->scan_time;
        lidatr_scan_2d.range_min = __scan_ptr->range_min;
        lidatr_scan_2d.range_max = __scan_ptr->range_max;
        for(int i=0; i<__scan_ptr->ranges.size(); ++i){
            // 雷达原始坐标系
            float angle = __scan_ptr->angle_min + i*__scan_ptr->angle_increment;
            float laser_x = __scan_ptr->ranges[i] * cos(angle);
            float laser_y = __scan_ptr->ranges[i] * sin(angle);
            // tf2::Transform map_pose = latest_tf_.inverse() * odom_pose_tf2;
            tf2::Vector3 point_in(laser_x, laser_y, 0);
            tf2::Vector3 point_out = base2laser_tf * point_in;
            // 转换为机器人坐标系
            float base_x = point_out.getX();
            float base_y = point_out.getY();
            // 构造Lidar2D
            Lidar2D lidar_2d;
            lidar_2d.idx = i;
            lidar_2d.angle = atan2(base_y, base_x);
            lidar_2d.range = hypot(base_x, base_y);
            lidar_2d.laser_pose.x = base_x;
            lidar_2d.laser_pose.y = base_y;
            if(!__scan_ptr->intensities.empty())
                lidar_2d.intensitie = __scan_ptr->intensities[i];
            lidar_2d.stamp = __scan_ptr->header.stamp + ros::Duration(__scan_ptr->time_increment);
            lidatr_scan_2d.scan_data.emplace_back(lidar_2d);
            PublishLidarScan2D(lidatr_scan_2d);
        }

        // 获取机器人当前位姿和目标点位姿
        Pose2D robot_pose;   //默认0,0  base_link原点
        Pose2D goal_pose;
        Velocity2D cmd_vel;
        if(__goal_pose_ptr){
            __planner_ptr->ComputeTwist(cmd_vel, lidatr_scan_2d, robot_pose, msg_convert::Pose2Pose2D(__goal_pose_ptr->pose));
        }

    }

}


LocalPlanner::~LocalPlanner(){

}


// Rviz位姿回调函数
void LocalPlanner::RvizPosesCallback(const geometry_msgs::PoseStampedConstPtr& msg){
    __goal_pose_ptr = msg;
    ROS_INFO("[RvizPosesCallback] get pose, x=%0.3f, y=%0.3f, yaw=%0.3f.", 
        msg->pose.position.x, 
        msg->pose.position.y,
        tf2::getYaw(msg->pose.orientation)
    );
    __goal_pose_pub.publish(*msg);
}


// 激光雷达回调函数
void LocalPlanner::LaserScanCallback(const sensor_msgs::LaserScanConstPtr& msg){
    __scan_ptr = msg;
}

// 发布LidarScan2D
void LocalPlanner::PublishLidarScan2D(LidarScan2D lidar_scan_2d){
    sensor_msgs::LaserScan scan_msg;
    scan_msg.header.frame_id = lidar_scan_2d.frame_id;
    scan_msg.header.stamp = lidar_scan_2d.time_stamp;
    scan_msg.angle_min = lidar_scan_2d.angle_min;
    scan_msg.angle_max = lidar_scan_2d.angle_max;
    scan_msg.angle_increment = lidar_scan_2d.angle_increment;
    scan_msg.time_increment = lidar_scan_2d.time_increment;
    scan_msg.scan_time = lidar_scan_2d.scan_time;
    scan_msg.range_min = lidar_scan_2d.range_min;
    scan_msg.range_max = lidar_scan_2d.range_max;
    for(auto lidar_2d : lidar_scan_2d.scan_data){
        scan_msg.ranges.emplace_back(lidar_2d.range);
        scan_msg.intensities.emplace_back(lidar_2d.intensitie);

        scan_msg.range_min = min(lidar_scan_2d.range_min, lidar_2d.range);
        scan_msg.range_max = max(lidar_scan_2d.range_min, lidar_2d.range);
    }
    __base_scan_pub.publish(scan_msg);
}



/******************************************************
 * 主函数 * 
 ******************************************************/
int main(int argc, char** argv){
    ros::init(argc, argv, "test_local_planner");
    setlocale(LC_ALL,"");   //ROS_INFO 中文显示, 需要添加头文件#include <sstream>
    tf2_ros::Buffer tfBuffer(::ros::Duration(10));
    tf2_ros::TransformListener tfListener(tfBuffer);

    ROS_INFO("test_local_planner initializating......");
    ros::NodeHandle nh;
    LocalPlanner local_planner(&tfBuffer);
    ros::spin();
    return 0;
}